﻿
// 2021/12/2: 增加俯仰角速度和横滚角速度。基于gnssimu-sample-v6.h (版本2020/11/11)
// 2021/12/16: 增加水平误差和垂直误差
// 2022/3/16: 增加行驶曲率
// 2022/11/2: 支持Spadas8.4
// 2022/11/14: 实现插值
// 2023/10/11: 补充部分英文注释
// 2023/11/14: 增加TAI时间类型。增加东北天向速度获取函数enuSpeed
// 2024/7/3: 基于新样本框架。枚举改为使用enum class
// 2025/8/4: 不使用cat宏
// 2025/8/4: 改为继承SessionSample。改回单头文件

#ifndef GNSSIMU_SAMPLE_V7_H
#define GNSSIMU_SAMPLE_V7_H

#include "spadas.h"

namespace gnssimu_sample_v7
{
	using namespace spadas;

	enum class GnssImuLocationMode
	{
		NoLocation = 0, // 无位置信息
		Normal = 1, // 默认模式
		RTKFixed = 2, // RTK固定解
		RTKFloat = 3, // RTK浮动解
		RTD = 4, // RTD
		IMUOnly = 5, // 仅惯导
		Modified = 6, // 后期修正
	};

	enum class GnssTimeType
	{
		UTC = 0, // UTC时间
		GPS = 1, // GPS时间
		TAI = 2, // TAI时间
	};

	struct GnssImuSampleData
	{
		GnssImuLocationMode locationMode; // 位置模式
		OptionalInt satelliteCount; // 卫星数量

		OptionalDouble longitude; // [°E] Longitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的经度
		OptionalDouble latitude; // [°N] Latitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的纬度
		OptionalDouble altitude; // [m] Altitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的海拔

		OptionalDouble antennaLongitude; // [°E] Longitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的经度
		OptionalDouble antennaLatitude; // [°N] Latitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的纬度
		OptionalDouble antennaAltitude; // [m] 天线位置海拔Altitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的海拔

		OptionalDouble horizontalError; // [m] 水平位置误差
		OptionalDouble verticalError; // [m] 垂直位置误差

		OptionalDouble speed; // [kph] 车速

		OptionalDouble orientation; // [deg -180~180] 0 as north, CCW is positive / 朝向 CCW为正 北为0
		OptionalDouble pitch; // [deg] Heading down is positive / 俯仰角 车头朝下为正
		OptionalDouble roll; // [deg] Right side heading down is positive / 横滚角 右侧朝下为正

		OptionalDouble yawRate; // [deg/s] Turning left is positive / 横摆角速度 左转为正
		OptionalDouble pitchRate; // [deg/s] Heading down is positive / 俯仰角速度 车头朝下为正
		OptionalDouble rollRate; // [deg/s] Right side heading down is positive / 横滚角速度 右侧朝下为正

		OptionalDouble ax; // [m/s2] Front side is positive / 纵向加速度 朝前为正
		OptionalDouble ay; // [m/s2] Left side is positive / 横向加速度 朝左为正
		OptionalDouble az; // [m/s2] Top side is positive / 天向加速度 朝上为正

		OptionalDouble curvature; // [1/m] Turning left is positive / 行驶曲率 左转为正

		OptionalDouble slipAngle; // [deg] Turning left is positive / 侧偏角 左转时为正
		OptionalDouble jerkX; // [m/s3] Front side is positive / 纵向急动度  朝前为正
		OptionalDouble jerkY; // [m/s3] Left side is positive / 横向急动度 朝左为正

		OptionalDouble arrivalTime; // [s] Arrival time offset (Relative to the session) / 数据到达时间戳（相对Session开始时间）
		GnssTimeType gnssTimeType; // 卫星时间类型
		Optional<TimeWithMS> gnssTime; // 卫星时间

		GnssImuSampleData()
		{
			locationMode = GnssImuLocationMode::NoLocation;
			gnssTimeType = GnssTimeType::UTC;
		}

		Optional<TimeWithMS> gnssTimeUTC()
		{
			if (gnssTime.isValid())
			{
				if (gnssTimeType == GnssTimeType::UTC) return gnssTime;
				else if (gnssTimeType == GnssTimeType::GPS) return gnssTime.value() + (-18000);
				else if (gnssTimeType == GnssTimeType::TAI) return gnssTime.value() + (-37000);
			}
			return Optional<TimeWithMS>();
		}

		Optional<Vector3D> enuSpeed() // [kph]
		{
			if (speed.valid && orientation.valid && pitch.valid)
			{
				Vector3D v;
				v.z = -speed.value * math::sin(pitch.value);
				Double hSpeed = speed.value * math::cos(pitch.value);
				v.y = hSpeed * math::cos(orientation.value);
				v.x = -hSpeed * math::sin(orientation.value);
				return v;
			}
			else return Optional<Vector3D>();
		}
	};

	class GnssImuSample : public SessionSample
	{
	private:
		class GnssImuSampleVars : public SessionSampleVars
		{
		public:
			SPADAS_VARS(GnssImuSample, SessionSampleVars)
			GnssImuSampleData data;

			Bool supportInterpolation() override
			{
				return TRUE;
			}

			SessionGeneralSample toGeneralSample()
			{
				SessionGeneralSample sample;
				sample.timestamp() = timestamp;

				Array<GeneralElement> values(40);
				sample.values() = values;

				values[0] = (Double)(Int)data.locationMode;
				values[1] = data.longitude;
				values[2] = data.latitude;
				values[3] = data.altitude;
				values[4] = data.speed;
				values[5] = data.orientation;
				values[6] = data.pitch;
				values[7] = data.roll;
				values[8] = data.yawRate;
				values[9] = data.ax;
				values[10] = data.ay;
				values[11] = data.az;
				values[12] = data.slipAngle;
				values[13] = data.jerkX;
				values[14] = data.jerkY;
				values[15] = data.satelliteCount.valid ? data.satelliteCount.value : OptionalDouble();
				values[16] = data.antennaLongitude;
				values[17] = data.antennaLatitude;
				values[18] = data.antennaAltitude;
				values[19] = (Double)(Int)data.gnssTimeType;
				values[20] = data.arrivalTime;
				values[21] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.year : OptionalDouble();
				values[22] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.month : OptionalDouble();
				values[23] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.day : OptionalDouble();
				values[24] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.hour : OptionalDouble();
				values[25] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.minute : OptionalDouble();
				values[26] = data.gnssTime.isValid() ? data.gnssTime.refValue().dateTime.second : OptionalDouble();
				values[27] = data.gnssTime.isValid() ? (Double)data.gnssTime.refValue().milliseconds : OptionalDouble();
				values[28] = data.pitchRate;
				values[29] = data.rollRate;
				values[30] = data.horizontalError;
				values[31] = data.verticalError;
				values[32] = data.curvature;

				sample.significantCount() = values.size();
				return sample;
			}

			Bool fromSample(String protocol, SessionSample sample) override
			{
				SessionGeneralSample generalSample;
				if (!sample.is<SessionGeneralSample>(generalSample)) return FALSE;

				timestamp = generalSample.timestamp();
				auto values = generalSample.values();

				if (protocol == "gnssimu-sample-v7")
				{
					if (values.size() != 40) return FALSE;

					data.locationMode = values[0].valid ? (GnssImuLocationMode)(Int)values[0].value : GnssImuLocationMode::NoLocation;
					data.longitude = values[1];
					data.latitude = values[2];
					data.altitude = values[3];
					data.speed = values[4];
					data.orientation = values[5];
					data.pitch = values[6];
					data.roll = values[7];
					data.yawRate = values[8];
					data.ax = values[9];
					data.ay = values[10];
					data.az = values[11];
					data.slipAngle = values[12];
					data.jerkX = values[13];
					data.jerkY = values[14];
					data.satelliteCount = values[15].valid ? (Int)values[15].value : OptionalInt();
					data.antennaLongitude = values[16];
					data.antennaLatitude = values[17];
					data.antennaAltitude = values[18];

					data.gnssTimeType = values[19].valid ? (GnssTimeType)(Int)values[19].value : GnssTimeType::UTC;
					data.arrivalTime = values[20];
					if (values[21].valid &&
						values[22].valid &&
						values[23].valid &&
						values[24].valid &&
						values[25].valid &&
						values[26].valid &&
						values[27].valid)
					{
						data.gnssTime = TimeWithMS((UInt)values[21].value,
							(UInt)values[22].value,
							(UInt)values[23].value,
							(UInt)values[24].value,
							(UInt)values[25].value,
							(UInt)values[26].value,
							(UInt)values[27].value);
					}
					else
					{
						data.gnssTime = Optional<TimeWithMS>();
					}

					data.pitchRate = values[28];
					data.rollRate = values[29];
					data.horizontalError = values[30];
					data.verticalError = values[31];
					data.curvature = values[32];

					return TRUE;
				}
				else if (protocol == "gnssimu-sample-v6")
				{
					if (values.size() != 28) return FALSE;

					data.locationMode = values[0].valid ? (GnssImuLocationMode)(Int)values[0].value : GnssImuLocationMode::NoLocation;
					data.longitude = values[1];
					data.latitude = values[2];
					data.altitude = values[3];
					data.speed = values[4];
					data.orientation = values[5];
					data.pitch = values[6];
					data.roll = values[7];
					data.yawRate = values[8];
					data.ax = values[9];
					data.ay = values[10];
					data.az = values[11];
					data.slipAngle = values[12];
					data.jerkX = values[13];
					data.jerkY = values[14];
					data.satelliteCount = values[15].valid ? (Int)values[15].value : OptionalInt();
					data.antennaLongitude = values[16];
					data.antennaLatitude = values[17];
					data.antennaAltitude = values[18];

					data.gnssTimeType = values[19].valid ? (GnssTimeType)(Int)values[19].value : GnssTimeType::UTC;
					data.arrivalTime = values[20];
					if (values[21].valid &&
						values[22].valid &&
						values[23].valid &&
						values[24].valid &&
						values[25].valid &&
						values[26].valid &&
						values[27].valid)
					{
						data.gnssTime = TimeWithMS((UInt)values[21].value,
							(UInt)values[22].value,
							(UInt)values[23].value,
							(UInt)values[24].value,
							(UInt)values[25].value,
							(UInt)values[26].value,
							(UInt)values[27].value);
					}
					else
					{
						data.gnssTime = Optional<TimeWithMS>();
					}

					return TRUE;
				}
				else if (protocol == "gnssimu-sample-v5")
				{
					if (values.size() != 19) return FALSE;

					data.locationMode = values[0].valid ? (GnssImuLocationMode)(Int)values[0].value : GnssImuLocationMode::NoLocation;
					data.longitude = values[1];
					data.latitude = values[2];
					data.antennaAltitude = values[3];
					data.speed = values[4];
					data.orientation = values[5];
					data.pitch = values[6];
					data.roll = values[7];
					data.yawRate = values[8];
					data.ax = values[9];
					data.ay = values[10];
					data.az = values[11];

					data.gnssTimeType = GnssTimeType::UTC;
					data.arrivalTime = timestamp.offset;
					if (values[12].valid &&
						values[13].valid &&
						values[14].valid &&
						values[15].valid &&
						values[16].valid &&
						values[17].valid &&
						values[18].valid)
					{
						data.gnssTime = TimeWithMS((UInt)values[12].value,
							(UInt)values[13].value,
							(UInt)values[14].value,
							(UInt)values[15].value,
							(UInt)values[16].value,
							(UInt)values[17].value,
							(UInt)values[18].value);
					}
					else
					{
						data.gnssTime = Optional<TimeWithMS>();
					}

					return TRUE;
				}
				else if (protocol == "gnssimu-sample-v4")
				{
					if (values.size() != 18) return FALSE;

					data.longitude = values[0];
					data.latitude = values[1];
					data.antennaAltitude = values[2];
					data.speed = values[3];
					data.orientation = values[4];
					data.pitch = values[5];
					data.roll = values[6];
					data.yawRate = values[7];
					data.ax = values[8];
					data.ay = values[9];
					data.az = values[10];

					data.locationMode = data.longitude.valid && data.latitude.valid ? GnssImuLocationMode::NoLocation : GnssImuLocationMode::NoLocation;

					data.gnssTimeType = GnssTimeType::UTC;
					data.arrivalTime = timestamp.offset;
					if (values[11].valid &&
						values[12].valid &&
						values[13].valid &&
						values[14].valid &&
						values[15].valid &&
						values[16].valid &&
						values[17].valid)
					{
						data.gnssTime = TimeWithMS((UInt)values[11].value,
							(UInt)values[12].value,
							(UInt)values[13].value,
							(UInt)values[14].value,
							(UInt)values[15].value,
							(UInt)values[16].value,
							(UInt)values[17].value);
					}
					else
					{
						data.gnssTime = Optional<TimeWithMS>();
					}

					return TRUE;
				}

				return FALSE;
			}

			SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
			{
				GnssImuSample gnssImuSample1, gnssImuSample2;
				if (!ss1.is<GnssImuSample>(gnssImuSample1) || !ss2.is<GnssImuSample>(gnssImuSample2)) return SessionSample();

				GnssImuSampleData& s1 = gnssImuSample1.var()->data;
				GnssImuSampleData& s2 = gnssImuSample2.var()->data;

				GnssImuSample gnssImuSample;
				gnssImuSample.timestamp() = timestamp;
				
				GnssImuSampleData& output = gnssImuSample.var()->data;
				if (s1.locationMode == s2.locationMode)
				{
					output.locationMode = s1.locationMode;
					output.longitude = s1.longitude * w1 + s2.longitude * w2;
					output.latitude = s1.latitude * w1 + s2.latitude * w2;
					output.altitude = s1.altitude * w1 + s2.altitude * w2;
					output.antennaLongitude = s1.antennaLongitude * w1 + s2.antennaLongitude * w2;
					output.antennaLatitude = s1.antennaLatitude * w1 + s2.antennaLatitude * w2;
					output.antennaAltitude = s1.antennaAltitude * w1 + s2.antennaAltitude * w2;
				}
				else
				{
					output.locationMode = w1 > w2 ? s1.locationMode : s2.locationMode;
					output.longitude = w1 > w2 ? s1.longitude : s2.longitude;
					output.latitude = w1 > w2 ? s1.latitude : s2.latitude;
					output.altitude = w1 > w2 ? s1.altitude : s2.altitude;
					output.antennaLongitude = w1 > w2 ? s1.antennaLongitude : s2.antennaLongitude;
					output.antennaLatitude = w1 > w2 ? s1.antennaLatitude : s2.antennaLatitude;
					output.antennaAltitude = w1 > w2 ? s1.antennaAltitude : s2.antennaAltitude;
				}

				output.satelliteCount = w1 > w2 ? s1.satelliteCount : s2.satelliteCount;

				output.speed = s1.speed * w1 + s2.speed * w2;
				output.orientation = interpolateAngle(s1.orientation, w1, s2.orientation, w2);
				output.pitch = s1.pitch * w1 + s2.pitch * w2;
				output.roll = s1.roll * w1 + s2.roll * w2;
				output.yawRate = s1.yawRate * w1 + s2.yawRate * w2;
				output.ax = s1.ax * w1 + s2.ax * w2;
				output.ay = s1.ay * w1 + s2.ay * w2;
				output.az = s1.az * w1 + s2.az * w2;
				output.slipAngle = s1.slipAngle * w1 + s2.slipAngle * w2;
				output.jerkX = s1.jerkX * w1 + s2.jerkX * w2;
				output.jerkY = s1.jerkY * w1 + s2.jerkY * w2;

				output.gnssTimeType = s1.gnssTimeType;
				output.arrivalTime = s1.arrivalTime * w1 + s2.arrivalTime * w2;
				if (s1.gnssTime.isValid() && s2.gnssTime.isValid())
				{
					Long diffMS = s2.gnssTime.value() - s1.gnssTime.value();
					output.gnssTime = s1.gnssTime.value() + (Long)(diffMS * w2 / (w1 + w2));
				}

				output.pitchRate = s1.pitchRate * w1 + s2.pitchRate * w2;
				output.rollRate = s1.rollRate * w1 + s2.rollRate * w2;
				output.horizontalError = s1.horizontalError * w1 + s2.horizontalError * w2;
				output.verticalError = s1.verticalError * w1 + s2.verticalError * w2;
				output.curvature = s1.curvature * w1 + s2.curvature * w2;

				return gnssImuSample.as<SessionSample>();
			}

			OptionalDouble interpolateAngle(OptionalDouble a1, Double w1, OptionalDouble a2, Double w2)
			{
				if (!a1.valid || !a2.valid) return OptionalDouble();

				Double x1 = math::cos(a1.value);
				Double y1 = math::sin(a1.value);
				Double x2 = math::cos(a2.value);
				Double y2 = math::sin(a2.value);

				Double xo = x1 * w1 + x2 * w2;
				Double yo = y1 * w1 + y2 * w2;
				if (xo == 0 && yo == 0) return OptionalDouble();

				return math::atan(yo, xo);
			}
		};
	public:
		SPADAS_CLASS("gnssimu_sample_v7.GnssImuSample", GnssImuSampleVars)

		GnssImuSample() : SessionSample(new GnssImuSampleVars)
		{}
		GnssImuSampleData *operator ->()
		{
			return &var()->data;
		}

		static String getProtocolName(UInt channel)
		{
			return (String)"gnssimu-sample-v7@" + channel;
		}
		SessionGeneralSample toGeneralSample()
		{
			return var()->toGeneralSample();
		}

		static Array<String> getProtocolNames(UInt channel)
		{
			return Array<String>::create(4, "gnssimu-sample-v4", "gnssimu-sample-v5", "gnssimu-sample-v6", "gnssimu-sample-v7").convert<String>([channel](auto& s){ return s + "@" + channel; });
		}
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample)
		{
			return fromSample(protocol, generalSample);
		}
	};
}

#endif
